#include "control.h"
#include "oled_ssd1306.h"

uint8_t RobotMode = MONITOR_MODE;

extern float Theta0, Theta1, Theta2, Theta3;
extern float MOTOR_ANGLE;

bool strnum_anlyse(unsigned char *str, unsigned short *shunxu, unsigned short *tar)
{
	uint16_t count = 0;
	uint16_t num = 0;
	if (str[*shunxu + 1] >= '0' && str[*shunxu + 1] <= '9')
	{
		for (uint16_t i = 2; str[*shunxu + i] >= '0' && str[*shunxu + i] <= '9'; i++)
		{
			count++;
		}
		for (uint16_t i = 1, j = count; i <= count + 1; i++, j--)
		{
			num += ((str[*shunxu + i] - 48) * pow(10, j));
		}
		(*shunxu) += (count + 2);
		*tar = num;
		return 1;
	}
	return 0;
}

bool Conditional_judgment(unsigned char *str, unsigned short *shunxu, unsigned short *flag)
{
	uint16_t tar_num = 0;
	if (str[*shunxu] == 'g')
	{
		if (str[*shunxu + 1] == 'm') //前方障碍物小于,gm数字
		{
			*shunxu += 1;
			if (strnum_anlyse(str, shunxu, &tar_num) == 1)
			{
				// cout << "get_distance1(" << tar_num << ")\n";
				if (1) //判断条件是否成立
				{
					*flag = 1;
				}
				else
				{
					*flag = 0;
				}
			}
		}
		else if (str[*shunxu + 1] == 'd') //前方障碍物大于,gd数字
		{
			*shunxu += 1;
			if (strnum_anlyse(str, shunxu, &tar_num) == 1)
			{
				// cout << "get_distance2(" << tar_num << ")\n";
				if (0) //判断if条件是否成立
				{
					*flag = 1;
				}
				else
				{
					*flag = 0;
				}
			}
		}
	}
	else
		return 0;
	return 1;
}

bool str_analyse(unsigned char *str)
{
	printf("str:%s\n", str);
	uint16_t str_length = 0; //字符串的长度
	str_length = strlen((const char *)str);
	uint16_t shunxu = 5;  //识别字符的顺序
	uint16_t tar_num = 0; //字符后面的数字
	uint8_t ifflag = 1;	  //判断if条件是否成立
	uint8_t whileflag = 1;
	uint8_t whilegeshi = 0; //判断while格式是否正确
	if (RobotMode == PROGRAM_MODE)
	{
		if (str[0] == 's' && str[1] == 't' && str[2] == 'a' && str[3] == 'r' && str[4] == 't' && str[str_length - 1] == 'd' && str[str_length - 2] == 'n' && str[str_length - 3] == 'e')
		{
			while (shunxu != str_length - 3)
			{
				/*****************************************功能****************************************/
				if (ifflag == 1 || whileflag == 1)
				{
					if (str[shunxu] == 't' && ifflag == 1) //设定小车速度,t数字
					{
						if (strnum_anlyse(str, &shunxu, &tar_num) == 1)
						{
							// cout << "set_speed(" << tar_num << ")\n";
							//  cout << "shunxu: " << shunxu << "\n";
						}
						else
						{
							// cout << "t格式错误\n";
							return 0;
						}
					}
					else if (str[shunxu] == 'f') //向前
					{
						// cout << "move_forward()"
						//	 << "\n";
						shunxu += 1;
					}
					else if (str[shunxu] == 'b') //向后
					{
						// cout << "move_back()"
						//	 << "\n";
						shunxu += 1;
					}
					else if (str[shunxu] == 'm')
					{
						if (str[shunxu + 1] == 'l') //小车向左转ml数字
						{
							shunxu += 1;
							if (strnum_anlyse(str, &shunxu, &tar_num) == 1)
							{
								// cout << "move_left(" << tar_num << ")\n";
							}
						}
						else if (str[shunxu + 1] == 'r') //小车向右转mr数字
						{
							shunxu += 1;
							if (strnum_anlyse(str, &shunxu, &tar_num) == 1)
							{
								// cout << "move_right(" << tar_num << ")\n";
							}
						}
						else
						{
							// cout << "m格式错误\n";
							return 0;
						}
					}
					else if (str[shunxu] == 'a')
					{
						if (str[shunxu + 1] == 'l') //机械臂向左转al数字
						{
							shunxu += 1;
							if (strnum_anlyse(str, &shunxu, &tar_num) == 1)
							{
								// cout << "arm_left(" << tar_num << ")\n";
							}
						}
						else if (str[shunxu + 1] == 'r') //机械臂向右转ar数字
						{
							shunxu += 1;
							if (strnum_anlyse(str, &shunxu, &tar_num) == 1)
							{
								// cout << "arm_right(" << tar_num << ")\n";
							}
						}
						else if (str[shunxu + 1] == 'u') //机械臂上升au数字
						{
							shunxu += 1;
							if (strnum_anlyse(str, &shunxu, &tar_num) == 1)
							{
								// cout << "arm_up(" << tar_num << ")\n";
							}
						}
						else if (str[shunxu + 1] == 'd') //机械臂下降ad数字
						{
							shunxu += 1;
							if (strnum_anlyse(str, &shunxu, &tar_num) == 1)
							{
								// cout << "arm_down(" << tar_num << ")\n";
							}
						}
						else
						{
							// cout << "a格式错误\n";
							return 0;
						}
					}
					else if (str[shunxu] == 'c') //夹起
					{
						// cout << "arm_clip()"
						//	 << "\n";
						shunxu += 1;
					}
					else if (str[shunxu] == 'r') //松开
					{
						// cout << "arm_release()"
						//	 << "\n";
						shunxu += 1;
					}
					else if (str[shunxu] == 'n') //识别数字,n数字
					{
						if (strnum_anlyse(str, &shunxu, &tar_num) == 1)
						{
							// cout << "get_num(" << tar_num << ")\n";
							//  cout << "shunxu: " << shunxu << "\n";
						}
						else
						{
							// cout << "n格式错误\n";
							return 0;
						}
					}
					else if (str[shunxu] == 'o') //识别颜色，o数字
					{
						if (strnum_anlyse(str, &shunxu, &tar_num) == 1)
						{
							// cout << "get_color(" << tar_num << ")\n";
							//  cout << "shunxu: " << shunxu << "\n";
						}
						else
						{
							// cout << "o格式错误\n";
							return 0;
						}
					}
					else if (str[shunxu] == 'd') //延时，d数字
					{
						if (strnum_anlyse(str, &shunxu, &tar_num) == 1)
						{
							// cout << "delay(" << tar_num << ")\n";
							//  cout << "shunxu: " << shunxu << "\n";
						}
						else
						{
							// cout << "d格式错误\n";
							return 0;
						}
					}
				}
				/*****************************************逻辑****************************************/
				if (str[shunxu] == 'i' && str[shunxu + 1] == 'f') // if
				{
					// cout << "if\n";
					shunxu += 2;
					if (Conditional_judgment(str, &shunxu, &ifflag) == 1)
					{
					}
					else
					{
						// cout << "if条件错误\n";
						return 0;
					}
				}
				else if (str[shunxu] == 'w' && str[shunxu + 1] == 'h' && str[shunxu + 2] == 'i' &&
						 str[shunxu + 3] == 'l' && str[shunxu + 4] == 'e') // while
				{
					// cout << "while\n";
					shunxu += 5;
					if (Conditional_judgment(str, &shunxu, &whileflag) == 1) //条件判断
					{
					}
					else
					{
						// cout << "while条件错误\n";
						return 0;
					}
					//对while的格式进行判断
					if (str[shunxu + 5] == '{')
					{
						shunxu += 1;
					}
					else
						whilegeshi = 0;
					for (uint16_t i = 0; shunxu + i < str_length - 3; i++)
					{
						if (str[shunxu + i] == '}')
						{
							whilegeshi = 1;
						}
					}
					if (whilegeshi == 0)
					{
						// cout << "while格式错误\n";
						return 0;
					}
				}
				else
				{
					// cout << "发送非法数据\n";
					return 0;
				}
			}
		}
		else
		{
			//向服务器发送信息，重新进行发送
			printf("包头包尾错误\n");
			return 0;
		}
	}
	else if (RobotMode == MONITOR_MODE)
	{
		if (strstr((const char *)str, "CAR_UP"))
		{
			move_forward();
			// OledFillScreen(0);
			OledShowString(20, 4, "CAR_UP", 1);
			osDelay(3);
		}
		else if (strstr((const char *)str, "CAR_DOWN"))
		{
			move_back();
			OledShowString(20, 5, "CAR_DOWN", 1);
			osDelay(3);
		}
		else if (strstr((const char *)str, "CAR_LEFT"))
		{
			MOTOR_ANGLE += 3;
		}
		else if (strstr((const char *)str, "CAR_RIGHT"))
		{
			MOTOR_ANGLE -= 3;
		}
		else if (strstr((const char *)str, "HAND_UP"))
		{
		}
		else if (strstr((const char *)str, "HAND_DOWN"))
		{
		}
		else if (strstr((const char *)str, "HAND_LEFT"))
		{
			Theta0 -= 3;
		}
		else if (strstr((const char *)str, "HAND_RIGHT"))
		{
			Theta0 += 3;
		}
		else
		{
			OledShowString(20, 3, "NO", 1);
		}
		move_stop();
	}
	else
	{
		printf("没有匹配的模式\n");
	}
	return 1;
}
